#include "imu_controller.hpp"
#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/msg/imu.hpp"
#include "hb_msgs/msg/heart_beat.hpp"

class imu_node : public rclcpp::Node
{
  private:
	std::string group_ip;
	int port;
	std::string remote_ip;
	std::string local_ip;
	int device_id;
	std::shared_ptr<imu_controller> controller;
	std::thread recv_thread;
	rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr imu_pub;
	rclcpp::Publisher<hb_msgs::msg::HeartBeat>::SharedPtr hb_pub;
	int error_code = 0;
	rclcpp::TimerBase::SharedPtr hb_timer;

	void recv_thread_func()
	{
		sensor_msgs::msg::Imu msg;
		int state = 0;
		int x, y, z, w;
		int timeout_cnt = 0;
		RCLCPP_DEBUG(this->get_logger(), "thread running");
		msg.header.stamp = rclcpp::Clock().now();
		msg.header.frame_id = "imu";
		while (rclcpp::ok())
		{
			auto frame_type = this->controller->read_frame();
			switch (frame_type)
			{
				case imu_frame_id::accel:
					// should receive this first
					this->controller->get_data_as_accel(x, y, z);
					if (state != 0)
					{
						RCLCPP_WARN(this->get_logger(), "uncomplete msg");
						this->error_code = -2;
					}

					msg.header.stamp = rclcpp::Clock().now();
					msg.header.frame_id = "imu";
					state = 1;
					msg.linear_acceleration.x = x * 9.8 / 1000;
					msg.linear_acceleration.y = y * 9.8 / 1000;
					msg.linear_acceleration.z = z * 9.8 / 1000;
					RCLCPP_DEBUG(this->get_logger(), "accel: %d %d %d", x, y, z);
					break;

				case imu_frame_id::angula:
					// should receive this after a accel frame
					this->controller->get_data_as_angula(x, y, z);
					if (state != 1)
					{
						RCLCPP_WARN(this->get_logger(), "unexpected angula frame");
						break;
					}

					state = 2;
					msg.angular_velocity.x = x * 0.1 * 3.14159 / 180;
					msg.angular_velocity.y = y * 0.1 * 3.14159 / 180;
					msg.angular_velocity.z = z * 0.1 * 3.14159 / 180;
					RCLCPP_DEBUG(this->get_logger(), "aungula: %d %d %d", x, y, z);
					break;

				case imu_frame_id::euler:
					// should receive this after an angula frame
					this->controller->get_data_as_euler(x, y, z);
					RCLCPP_DEBUG(this->get_logger(), "euler: %d %d %d", x, y, z);
					break;

				case imu_frame_id::quad:
					// should receive this after an euler frame
					this->controller->get_data_as_quad(w, x, y, z);
					if (state != 2)
					{
						RCLCPP_WARN(this->get_logger(), "unexpected angula frame");
						break;
					}

					state = 3;
					msg.orientation.w = w / 10000.0;
					msg.orientation.x = x / 10000.0;
					msg.orientation.y = y / 10000.0;
					msg.orientation.z = z / 10000.0;
					RCLCPP_DEBUG(this->get_logger(), "quad: %d %d %d %d", w, x, y, z);
					break;

				case imu_frame_id::pascal:
					// not enabled, not needed
					this->controller->get_data_as_pascal(x);
					RCLCPP_DEBUG(this->get_logger(), "pascal: %d", x);
					break;

				case imu_frame_id::none:
					// a timeout
					if (timeout_cnt++ > 50)
					{
						RCLCPP_WARN(this->get_logger(), "timeout, no frame was receieved");
						timeout_cnt = 0;
						this->error_code = -1; // timeout
					}

					break;

				default:
					RCLCPP_DEBUG(this->get_logger(), "unknown frame (%x)", frame_type);
					break;
			}
			if (state == 3)
			{
				this->imu_pub->publish(msg);
				RCLCPP_DEBUG(this->get_logger(), "published imu msg");
				state = 0;
				this->error_code = 0;
			}
		}
	}

	void hb_callback()
	{
		//send heart beat message at given frequency
		hb_msgs::msg::HeartBeat hb;
		hb.name = this->get_fully_qualified_name();
		hb.stamp = rclcpp::Clock().now();
		switch (this->error_code)
		{
			case 0:
				hb.code = 0;
				hb.reason = "ok";
				this->error_code = -1;
				break;
			case -1:
				hb.code = -1;
				hb.reason = "timeout, got no frame from imu";
				break;
			case -2:
				hb.code = -2;
				hb.reason = "message uncomplete";
				this->error_code = -1;
			default:
				hb.code = 0;
				hb.reason = "ok";
		}
		this->hb_pub->publish(hb);
	}

  public:
	imu_node(std::string name) : Node(name)
	{
		// parameters
		this->declare_parameter<std::string>("group_ip", "239.255.0.6");
		this->declare_parameter<int>("port", 6781);
		this->declare_parameter<std::string>("remote_ip", "192.168.1.100");
		this->declare_parameter<std::string>("local_ip", "192.168.1.201");
		this->declare_parameter<int>("device_id", 8);

		this->get_parameter("group_ip", this->group_ip);
		this->get_parameter("port", this->port);
		this->get_parameter("remote_ip", this->remote_ip);
		this->get_parameter("local_ip", this->local_ip);
		this->get_parameter("device_id", this->device_id);

		RCLCPP_INFO(
			this->get_logger(),
			"in multicast group %s, port %d, remote host %s, local host %s, device id: %d",
			this->group_ip.c_str(), this->port, this->remote_ip.c_str(), this->local_ip.c_str(),
			this->device_id);

		auto sock = std::make_shared<iolayer::layer_socket>(this->group_ip, this->port,
															this->remote_ip, this->local_ip);
		if (sock->sock < 0)
		{
			RCLCPP_FATAL(this->get_logger(),
						 "failed to setup socket, please check launch parameter file");
			exit(-1);
		}
		this->controller = std::make_shared<imu_controller>(sock, this->device_id);
		this->controller->write_freq_accel(0x0A);
		this->controller->write_freq_angula(0x0A);
		this->controller->write_freq_quad(0x0A);
		this->controller->write_freq_euler(0);
		this->controller->write_freq_pascal(0);

		this->imu_pub = this->create_publisher<sensor_msgs::msg::Imu>("Imu_data", 20);
		this->hb_pub = this->create_publisher<hb_msgs::msg::HeartBeat>("/heartbeat", 20);
		this->hb_timer = this->create_wall_timer(std::chrono::milliseconds(200), std::bind(&imu_node::hb_callback, this));

		RCLCPP_INFO(this->get_logger(), "initialization done, start thread");
		this->recv_thread = std::thread(std::bind(&imu_node::recv_thread_func, this));
		this->recv_thread.detach();
	}
};

int main(int argc, char **argv)
{
	rclcpp::init(argc, argv);
	auto node = std::make_shared<imu_node>("imu_node");
	rclcpp::spin(node);
	rclcpp::shutdown();
	return 0;
}
